#include "tcpIrWorker.h"
#include "PointCloudGenerator.h"

#include <qtcpsocket.h>
#include <QThread>
#include <QImage>
#include <QFile>
#include <QCoreApplication>
// 定义一个函数来解析 YAML 文件中的键值对
QHash<QString, QVariant> TcpIrWorker::parseYaml(const QString& filePath) {
    QHash<QString, QVariant> result;
    QFile file(filePath);

    if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
        qDebug() << "无法打开文件";
        return result;
    }

    QTextStream in(&file);
    QString currentSection;
    while (!in.atEnd()) {
        QString line = in.readLine().trimmed();
        if (line.isEmpty() || line.startsWith('#')) {
            continue; // 跳过空行和注释
        }

        if (line.endsWith(':')) {
            // 这是一个节（section）的开始
            currentSection = line.left(line.length() - 1);
        }
        else if (line.contains(':')) {
            // 这是一个键值对
            QStringList parts = line.split(':');
            QString key = parts[0].trimmed();
            QString value = parts[1].trimmed();

            // 处理值的类型转换
            if (value.startsWith('"') && value.endsWith('"')) {
                // 字符串类型
                value = value.mid(1, value.length() - 2);
                result[key] = value;
            }
            else if (value == "true") {
                // 布尔类型（true）
                result[key] = true;
            }
            else if (value == "false") {
                // 布尔类型（false）
                result[key] = false;
            }
            else {
                // 尝试转换为整数或浮点数
                bool ok;
                int intValue = value.toInt(&ok);
                if (ok) {
                    result[key] = intValue;
                }
                else {
                    double doubleValue = value.toDouble(&ok);
                    if (ok) {
                        result[key] = doubleValue;
                    }
                }
            }
        }
    }

    file.close();
    return result;
}


TcpIrWorker::TcpIrWorker(QObject *parent)
    : QObject{parent}, m_socket(nullptr)
{

}
quint32 TcpIrWorker::convertHexStringToDecimal(const QString& hexStr) {
    // 1. 将十六进制字符串转换为字节数组
    QByteArray byteArray = QByteArray::fromHex(hexStr.toUtf8());

    // 2. 反转字节顺序（大端转小端）
    QByteArray reversedBytes = byteArray;
    std::reverse(reversedBytes.begin(), reversedBytes.end());

    // 3. 将反转后的字节数组转换为无符号32位整数
    quint32 result = 0;
    for (char byte : reversedBytes) {
        result = (result << 8) | static_cast<quint8>(byte);
    }

    return result;
}
QByteArray TcpIrWorker::hexStringToLittleEndian(const QString& hexString) {
    // 1. 将十六进制字符串转换为字节数组
    QByteArray byteArray = QByteArray::fromHex(hexString.toUtf8());

    // 2. 反转字节顺序（大端转小端）
    std::reverse(byteArray.begin(), byteArray.end());

    return byteArray;
}


/**
 * 将十六进制字符串转换为 cv::Mat
 * @param hexString 十六进制字符串（如 "EE0CD70CDA0C..."）
 * @param width 图像宽度
 * @param height 图像高度
 * @return 转换后的 cv::Mat (CV_16UC1 类型)
 */
cv::Mat TcpIrWorker::hexStringToDepthMat(const QString& hexString, int width, int height) {
    // 创建 16 位无符号整型的 cv::Mat
    cv::Mat depthMat(height, width, CV_16UC1);

    // 确保十六进制字符串长度正确（每 4 个字符表示一个 uint16）
    if (hexString.length() % 4 != 0 || hexString.length() != width * height * 4) {
        qDebug() << "十六进制字符串长度错误!";
        return cv::Mat();
    }

    // 逐像素解析
    for (int i = 0; i < hexString.length(); i += 4) {
        // 提取四个字符（如 "EE0C"）
        QString hexPair = hexString.mid(i, 4);

        // 交换字节顺序（大端转小端："EE0C" → "0CEE"）
        QString swappedHex = hexPair.mid(2, 2) + hexPair.mid(0, 2);

        // 将十六进制字符串转换为十进制数值
        bool ok;
        quint16 depthValue = swappedHex.toUShort(&ok, 16);

        if (!ok) {
            qDebug() << "转换失败，无效的十六进制值:" << hexPair;
            continue;
        }

        // 计算像素在图像中的位置
        int pixelIndex = i / 4;
        int row = pixelIndex / width;
        int col = pixelIndex % width;

        // 填充到 cv::Mat
        depthMat.at<quint16>(row, col) = depthValue;
    }

    return depthMat;
}



void TcpIrWorker::connectToServer(const QString &ip, quint16 port)
{
    if (m_socket && m_socket->state() == QAbstractSocket::ConnectedState)
    {
        //emit stopConnectDepthTimer();
    }
    if (m_ip == ip && m_port == port && m_socket && m_socket->state() == QAbstractSocket::ConnectedState) {
        qDebug() << "已经连接到目标服务器，无需重复连接";
        //return;
    }

    m_ip = ip;
    m_port = port;

    if (m_socket) {
        m_socket->deleteLater();
        m_socket = nullptr;
    }
    m_socket = new QTcpSocket();

    connect(m_socket, &QTcpSocket::connected, this, &TcpIrWorker::connected);
    connect(m_socket, &QTcpSocket::disconnected, this, &TcpIrWorker::disconnected);
    connect(m_socket, &QTcpSocket::readyRead, this, &TcpIrWorker::onReadyRead);
    
    m_socket->connectToHost(ip, port);

    if (m_socket->waitForConnected(5000)) {
        qDebug() << "连接成功";
    } else {
        emit errorOccurred(m_socket->errorString());
        qDebug() << "连接失败" << m_socket->errorString();
    }
}

void TcpIrWorker::sendData(const QByteArray &data)
{
    qDebug() << "TcpDepthWorker::sendData 当前线程：" << QThread::currentThreadId();
    qDebug() << "send Data" << data.toHex().toUpper();
    if (m_socket && m_socket->state() == QAbstractSocket::ConnectedState) {
        m_socket->write(data);
        m_socket->flush();
    } else {
        qWarning() << "Socket not connected. Cannot send data.";
    }
}

void TcpIrWorker::onDisconnectSocket()
{
    if (m_socket->state() == QAbstractSocket::ConnectedState) {
        // 主动断开连接
        m_socket->disconnectFromHost();

        // 等待连接完全关闭（可选，超时后强制关闭）
        if (!m_socket->waitForDisconnected(3000)) { // 超时3秒
            qDebug() << "断开连接超时，强制关闭";
            m_socket->abort(); // 超时后强制关闭
        }
    }
}



void TcpIrWorker::onReadyRead()
{
    qDebug() << "TcpDepthWorker::onReadyRead" << QThread::currentThreadId();
    //QByteArray data = m_socket->readAll();
    QSharedPointer<QByteArray> data(new QByteArray(m_socket->readAll()));
    emit newIrDataReceived(data);
    //processReceiveBuffer(data);

}
void TcpIrWorker::processReceiveBuffer(const QByteArray& data) {
    QByteArray temByteArr = data.toHex().toUpper();
    m_receiveBuffer.append(temByteArr);
    while (true)
    {
        int endIndex = m_receiveBuffer.indexOf("EEFFEEFF");
        if (endIndex == -1)
        {
            // 没有找到帧尾退出循环
            break;
        }
        int frameLength = endIndex + 8;
        if (m_receiveBuffer.size() >= frameLength)
        {
            // 提取完整的一帧
            QByteArray completeFrame = m_receiveBuffer.left(frameLength);
            // 提取剩余数据
            QByteArray remainingData = m_receiveBuffer.mid(frameLength);
            processCompleteFrame(completeFrame);
            m_receiveBuffer = remainingData;
        }
        else {
            // 数据不完整，退出循环
            break;
        }
    }
}

void TcpIrWorker::processCompleteFrame(QByteArray& data)
{

    //qDebug() << "222" << m_receiveBuffer.mid(40, 22);
    //qDebug() << "333" << m_receiveBuffer.mid(42, 22);
    //qDebug() << m_receiveBuffer.mid(0, 64);
    //qDebug() << m_receiveBuffer.mid(64, 2);
    //qDebug() << m_receiveBuffer.mid(66, 8);
    quint32 depthWidth = convertHexStringToDecimal(data.mid(66, 8));
    //qDebug() << m_receiveBuffer.mid(74, 8);
    quint32 depthHeight = convertHexStringToDecimal(data.mid(74, 8));

    // 01000000 代表quint8   02000000代表quint16
    //qDebug() << "11" << m_receiveBuffer.mid(82, 8);
    // 02000000 代表内存连续
    //qDebug() << "22" << m_receiveBuffer.mid(90, 8);
    // 代表数据长度
    //qDebug() << "33" << m_receiveBuffer.mid(98, 16);
    quint32 dataLength = convertHexStringToDecimal(data.mid(98, 16));

    data.remove(0, 114);
    QByteArray newByteArray;
    if (data.endsWith("EEFFEEFF"))
    {
        newByteArray = data.left(data.length() - 32);
    }
    QByteArray str = hexStringToLittleEndian(newByteArray);
    qDebug() << "str" << str.size();
    if (str.size() == depthWidth * depthHeight * 2)
    {
        // Format_Grayscale8 Format_RGB888
        QImage depthImage(depthWidth, depthHeight, QImage::Format_RGB888);
        parseDepthDataUint16(str, depthImage, depthWidth, depthHeight);
        emit depthImageReady(depthImage);

    newByteArray.clear();
    //m_receiveBuffer.clear();
    str.clear();
    }
}

// 解析uint16类型的深度数据
void TcpIrWorker::parseDepthDataUint16(const QByteArray & data, QImage & image, int width, int height) {
    const unsigned short* rawData = reinterpret_cast<const unsigned short*>(data.constData());
    int dataSize = data.size() / 2; // uint16是2字节

    // 计算最大深度值用于归一化
    unsigned short maxDepth = 0;
    for (int i = 0; i < qMin(width * height, dataSize); i++) {
        if (rawData[i] > maxDepth) {
            maxDepth = rawData[i];
        }
    }

    // 将深度值转换为灰度图像
    for (int y = 0; y < height; y++) {
        for (int x = 0; x < width; x++) {
            int index = y * width + x;
            if (index < dataSize) {
                unsigned short depth = rawData[index];
                // 将深度值映射到RGB颜色（0=黑色，255=白色）
                int gray = static_cast<int>(depth * 255.0 / maxDepth);
                image.setPixel(x, y, qRgb(gray, gray, gray));
            }
        }

    }
}


